﻿using System;
using System.Collections.Generic;
using System.Linq;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;

namespace WindowsGame3
{
    class Physics
    {
        public float h;
        Vector3 gravity;
        public Matrix3 angularVelocityStar;
        private Vector3 angularVelocity;
        public const int NOTHINGTOWORRYABOUT = 0;
        public const int COLLISION = 1;
        public const int INTERPENETRATION = 2;
        public int collisionState = NOTHINGTOWORRYABOUT;
        public float time = 0;
        float ContactThreshold = 0.1f;

        public void initPhysics(ref RigidBody rigBody, Vector3 forcePush, Vector3 pushPoint)
        {
            rigBody.density = 0.001f;
            h = 0.1f;

            angularVelocityStar = new Matrix3(1, 0, 0, 0, 1, 0, 0, 0, 1);
            angularVelocity = new Vector3();
            rigBody.rotation = new Matrix3(1, 0, 0, 0, 1, 0, 0, 0, 1);

            //gravity = new Vector3(0f, -9.82f, 0f);
            gravity = new Vector3(0f, 0f, 0f);
            rigBody.oldRotation = new Matrix3(1, 0, 0, 0, 1, 0, 0, 0, 1);

            rigBody.linearMomentum = h* forcePush;
            //rigBody.linearMomentum = new Vector3();
            rigBody.angularMomentum = h * Vector3.Cross(pushPoint, forcePush);
            //Console.WriteLine("LinearMomentum: " + rigBody.linearMomentum);
            //Console.WriteLine("AngularMomentum: " + rigBody.angularMomentum);
            for (int i = 0; i < rigBody.numberOfPolygons; i++)
            {
                calculateCentroid(ref rigBody.polygonArray[i]);
                calculatePolygonMass(ref rigBody.polygonArray[i], rigBody.density);
            }

            calculateObjectMass(ref rigBody);
            calculateInertiaTensorInverse(ref rigBody);
            calculateObjectCenterOfMass(ref rigBody);
            calculateNormals(ref rigBody);
        }

        public void updatePhysics(ref RigidBody rigBody, float timestep)
        {
            h = timestep;

            if (collisionState == NOTHINGTOWORRYABOUT)
            {
                rigBody.oldPosition = rigBody.position;
                rigBody.oldAngularMomentum = rigBody.angularMomentum;
                rigBody.oldLinearMomentum = rigBody.linearMomentum;
                rigBody.oldRotation = rigBody.rotation;

                calculateMomentum(ref rigBody);
                calculatePosition(ref rigBody);
                calculateRotation(ref rigBody);

            }
            else if (collisionState == COLLISION)
            {
               /* calculateMomentum(ref rigBody);
                calculatePosition(ref rigBody);
                calculateRotation(ref rigBody);

                rigBody.oldPosition = rigBody.position;
                rigBody.oldAngularMomentum = rigBody.angularMomentum;
                rigBody.oldLinearMomentum = rigBody.linearMomentum;
                rigBody.oldRotation = rigBody.rotation;*/
            }
            time += h;
        }

        public void stepBack(ref RigidBody rigBody)
        {
            rigBody.position = rigBody.oldPosition;
            rigBody.angularMomentum = rigBody.oldAngularMomentum;
            rigBody.linearMomentum = rigBody.oldLinearMomentum;
            rigBody.rotation = rigBody.oldRotation;
        }

        public VertexPositionColor[] localToWorldVPC(ref RigidBody rigBody, ref Polygon poly)
        {
            VertexPositionColor[] world = new VertexPositionColor[3];

            world[0].Position = rigBody.oldRotation * poly.vertices[0].Position + rigBody.oldPosition;
            world[1].Position = rigBody.oldRotation * poly.vertices[1].Position + rigBody.oldPosition;
            world[2].Position = rigBody.oldRotation * poly.vertices[2].Position + rigBody.oldPosition;
           return world;
        }

        public Vector3[] localToWorldV3(ref RigidBody rigBody, ref Polygon poly)
        {
            Vector3[] world = new Vector3[3];

            world[0] = rigBody.oldRotation * poly.vertices[0].Position + rigBody.oldPosition;
            world[1] = rigBody.oldRotation * poly.vertices[1].Position + rigBody.oldPosition;
            world[2] = rigBody.oldRotation * poly.vertices[2].Position + rigBody.oldPosition;
            return world;
        }

        public void calculateCentroid(ref Polygon triangle)
        {
            Vector3 centroid = new Vector3();

            for (int i = 0; i < triangle.vertices.Length; i++)
            {
                centroid.X += triangle.vertices[i].Position.X;
                centroid.Y += triangle.vertices[i].Position.Y;
                centroid.Z += triangle.vertices[i].Position.Z;
            }

            centroid.X = centroid.X / triangle.vertices.Length;
            centroid.Y = centroid.Y / triangle.vertices.Length;
            centroid.Z = centroid.Z / triangle.vertices.Length;

            triangle.centerOfMass = centroid;
            //System.Diagnostics.Debug.WriteLine("Centroid: " + centroid.X + ", " + centroid.Y + ", " + centroid.Z);
        }

        public void calculatePolygonMass(ref Polygon triangle, float density)
        {
            Vector3 AB = new Vector3();
            Vector3 AC = new Vector3();

            AB = Vector3.Subtract(triangle.vertices[0].Position, triangle.vertices[1].Position);
            AC = Vector3.Subtract(triangle.vertices[0].Position, triangle.vertices[2].Position);

            triangle.area = (float)(0.5 * (Vector3.Cross(AB, AC).Length()));
            triangle.mass = triangle.area * density;
        }

        public void calculateObjectMass(ref RigidBody rigBody)
        {
            float totalMass = 0.0f;

            for (int i = 0; i < rigBody.polygonArray.Length; i++)
            {
                totalMass += rigBody.polygonArray[i].mass;
            }
            rigBody.totalMass = totalMass;
            //Console.WriteLine("total mass: " + totalMass);
        }

        public void calculateObjectCenterOfMass(ref RigidBody rigBody)
        {
            Vector3 COM = new Vector3();

            for (int i = 0; i < rigBody.polygonArray.Length; i++)
            {
                COM += rigBody.polygonArray[i].centerOfMass * rigBody.polygonArray[i].mass;
            }

            COM = COM / rigBody.totalMass;
            rigBody.centerOfMass = COM;
            //Console.WriteLine("COM " + COM);

            //COM i origo
            for (int i = 0; i < rigBody.polygonArray.Length; i++)
            {
                rigBody.polygonArray[i].vertices[0].Position = rigBody.polygonArray[i].vertices[0].Position - COM;
                rigBody.polygonArray[i].vertices[1].Position = rigBody.polygonArray[i].vertices[1].Position - COM;
                rigBody.polygonArray[i].vertices[2].Position = rigBody.polygonArray[i].vertices[2].Position - COM;
            }

            for (int i = 0; i < rigBody.numberOfPolygons; i++)
            {
                calculateCentroid(ref rigBody.polygonArray[i]);

            }
        }

        public void calculateInertiaTensorInverse(ref RigidBody rigBody)
        {
            Matrix3 Ibody = new Matrix3();
            Ibody.M11 = rigBody.totalMass * ((float)Math.Pow(rigBody.height, 2.0) + (float)Math.Pow(rigBody.depth, 2.0)) / 12;
            Ibody.M22 = rigBody.totalMass * ((float)Math.Pow(rigBody.width, 2.0) + (float)Math.Pow(rigBody.depth, 2.0)) / 12;
            Ibody.M33 = rigBody.totalMass * ((float)Math.Pow(rigBody.height, 2.0) + (float)Math.Pow(rigBody.width, 2.0)) / 12;

            rigBody.inertiaTensorInverse = Ibody.Inverse();
        }

        public void calculatePosition(ref RigidBody rigBody)
        {
            rigBody.position = rigBody.oldPosition + h * rigBody.linearMomentum / rigBody.totalMass;
        }

        public void calculateRotation(ref RigidBody rigBody)
        {            
            rigBody.angularVelocity = rigBody.rotation * rigBody.inertiaTensorInverse * rigBody.rotation.Transverse() * rigBody.angularMomentum;

            angularVelocityStar.Star(rigBody.angularVelocity);
            rigBody.rotation = rigBody.oldRotation + h * angularVelocityStar * rigBody.rotation;
            rigBody.rotation = rigBody.rotation.GramSchmidt();
            //rigBody.angularVelocity = angularVelocity;
            //Console.WriteLine("Rotation: " + angularVelocity);
        }

        public void calculateMomentum(ref RigidBody rigBody)
        {
            Vector3 torque = new Vector3(0f, 0f, 0f);
            Vector3 forceI = new Vector3(0f, 0f, 0f);
            Vector3 totalForce = new Vector3(0f, 0f, 0f);
            for (int i = 0; i < rigBody.numberOfPolygons; i++)
            {
                forceI = calculateCentroidForce(rigBody.polygonArray[i]);
                totalForce += forceI;
                torque += Vector3.Cross(rigBody.oldRotation * rigBody.polygonArray[i].centerOfMass, forceI);
                //Console.WriteLine("torque: " + torque);
            }
            rigBody.linearMomentum = rigBody.oldLinearMomentum + h * totalForce;
            rigBody.angularMomentum = rigBody.oldAngularMomentum + h * torque;
            //Console.WriteLine("Linear Momentum: " + rigBody.linearMomentum);
            //Console.WriteLine("Angular Momentum: " + rigBody.angularMomentum);
        }
        // räknar ut polygonen triangles center of mass.
        public Vector3 calculateCentroidForce(Polygon poly)
        {
            return (Vector3)(gravity);
        }

        public void calculateNormals(ref RigidBody rigBody)
        {
            for (int i = 0; i < rigBody.numberOfPolygons; i++)
            {

                Vector3 a = rigBody.polygonArray[i].vertices[2].Position - rigBody.polygonArray[i].vertices[1].Position;
                Vector3 b = rigBody.polygonArray[i].vertices[1].Position - rigBody.polygonArray[i].vertices[0].Position;

                rigBody.polygonArray[i].normal = Vector3.Cross(b, a);
                rigBody.polygonArray[i].normal.Normalize();
            }
        }

        public void restingContact(ref Contact contact)
        {
            float[] separatingDistance = new float[contact.ContactPoints.Count()];
            for (int i = 0; i < contact.ContactPoints.Count(); i++)
            {
                separatingDistance[i] = Vector3.Dot(contact.normal, contact.couple.relativeVelocities[i]);


            }

             

        }

        public void collidingContact(ref Contact contact)
        {
            float elasticity = 0.0f;
            float Ma = contact.couple.rigBodyA.totalMass;
            float Mb = contact.couple.rigBodyB.totalMass;
            Vector3 n = contact.normal;
            Matrix3 invIA =contact.couple.rigBodyA.inertiaTensorInverse;
            Matrix3 invIB =contact.couple.rigBodyB.inertiaTensorInverse;
             
            for (int i = 0; i < contact.ContactPoints.Count(); i++)
            {
                double vRel = Vector3.Dot(n,contact.couple.relativeVelocities[i]);
                Vector3 rA = contact.ContactPoints[i] - contact.couple.rigBodyA.position;
                Vector3 rB = contact.ContactPoints[i] - contact.couple.rigBodyB.position;
                

                double numerator = -(1 + elasticity) * vRel;
                double term1 = 1 / Ma;
                double term2 = 1 / Mb;
                double term3 = Vector3.Dot(n , (Vector3.Cross((invIA * Vector3.Cross(rA, n)), rA)));
                double term4 = Vector3.Dot(n ,(Vector3.Cross((invIB * Vector3.Cross(rB, n)), rB)));
                double j = numerator / (term1 + term2 + term3 + term4);
                Vector3 J = (float)j * n;

                Vector3 deltaV = J / contact.couple.rigBodyA.totalMass;
                Vector3 torqueImpulse = Vector3.Cross(rA, J);
                Vector3 deltaAngularVelocity = invIA * torqueImpulse;


                contact.couple.rigBodyA.linearMomentum += J;
                contact.couple.rigBodyB.linearMomentum -= J;
                contact.couple.rigBodyA.angularMomentum += torqueImpulse;
                contact.couple.rigBodyB.angularMomentum -= torqueImpulse;
            }
            collisionState = NOTHINGTOWORRYABOUT;
        }

        public void collisionType(ref Contact contact)
        {
            contact.collisionType = new int[contact.ContactPoints.Count()];

            for (int i = 0; i < contact.couple.relativeVelocities.Count(); i++)
            {
                if (Vector3.Dot(contact.couple.relativeVelocities[i], contact.normal) < Contact.COLLIDINGTHRESHOLD)
                {
                    contact.collisionType[i] = Contact.COLLIDING;
                }
                else if (Vector3.Dot(contact.couple.relativeVelocities[i], contact.normal) > Contact.COLLIDINGTHRESHOLD)
                {
                    contact.collisionType[i] = 0;
                }
                else
                {
                    contact.collisionType[i] = Contact.RESTING;
                }
            }
        }

        public Boolean calculateSeparatingPlane(ref Couple couple)
        {
            for (int i = 0; i < couple.rigBodyA.numberOfPolygons; i++)
            {
                Vector3 normal = couple.rigBodyA.rotation * couple.rigBodyA.polygonArray[i].normal;
                normal.Normalize();
                int rightSideCheck = 0;
                for (int j = 0; j < couple.rigBodyB.numberOfPolygons; j++)
                {
                    for (int k = 0; k < 3; k++)
                    {
                        //A er en punkt i planet som testas, B är punkten man testar imot 
                        Vector3 A = couple.rigBodyA.rotation * couple.rigBodyA.polygonArray[i].vertices[k].Position + couple.rigBodyA.position;
                        Vector3 B = couple.rigBodyB.rotation * couple.rigBodyB.polygonArray[j].vertices[k].Position + couple.rigBodyB.position;
                        Vector3 AB = B - A;
                        if (Vector3.Dot(AB, normal) > 0)
                        { // no collision
                            rightSideCheck++;
                        }
                    }
                }
                if (rightSideCheck == couple.rigBodyB.numberOfPolygons * 3)
                { //separationsplan!!!
                    couple.faceOnA = true;
                    couple.planeExists = true;
                    couple.sepIndex = i;
                    couple.definingFace = couple.rigBodyA.polygonArray[i];
                    return true;
                }
            }

            for (int i = 0; i < couple.rigBodyB.numberOfPolygons; i++)
            {
                Vector3 normal = couple.rigBodyB.rotation * couple.rigBodyB.polygonArray[i].normal;
                normal.Normalize();
                int rightSideCheck = 0;
                Vector3 A = couple.rigBodyB.rotation * couple.rigBodyB.polygonArray[i].vertices[0].Position + couple.rigBodyB.position;
                for (int j = 0; j < couple.rigBodyA.numberOfPolygons; j++)
                {
                    for (int k = 0; k < 3; k++)
                    {
                        //A er en punkt i planet som testas, B är punkten man testar imot                       
                        Vector3 B = couple.rigBodyA.rotation * couple.rigBodyA.polygonArray[j].vertices[k].Position + couple.rigBodyA.position;
                        Vector3 AB = B - A;
                        if (Vector3.Dot(AB, normal) > 0)
                        { // no collision
                            rightSideCheck++;
                        }
                    }
                }
                if (rightSideCheck == couple.rigBodyA.numberOfPolygons * 3)
                { //separationsplan!!!
                    couple.faceOnA = false;
                    couple.planeExists = true;
                    couple.sepIndex = i;
                    couple.definingFace = couple.rigBodyB.polygonArray[i];
                    return true;
                }
            }
            collisionState = COLLISION;
            return false;
        }
     
        public void calculateRelativevelocity(ref Contact contact)
        {
            Vector3 velocityA = contact.couple.rigBodyA.oldLinearMomentum / contact.couple.rigBodyA.totalMass;
            Vector3 velocityB = contact.couple.rigBodyB.oldLinearMomentum / contact.couple.rigBodyB.totalMass;
            contact.couple.relativeVelocities = new List<Vector3>();

           for(int i=0; i<contact.ContactPoints.Count(); i++)
           {
               Vector3 pointVelocityA = velocityA + Vector3.Cross(contact.couple.rigBodyA.angularVelocity, (contact.ContactPoints[i]-contact.couple.rigBodyA.position));
               Vector3 pointVelocityB = velocityB + Vector3.Cross(contact.couple.rigBodyB.angularVelocity, (contact.ContactPoints[i] - contact.couple.rigBodyB.position));
               contact.couple.relativeVelocities.Add(pointVelocityA - pointVelocityB);
           }            
        }

        public Contact findContactPoints(ref Couple couple)
        {
            Contact contact = new Contact();
            contact.ContactPoints = new List<Vector3>();
            contact.couple = couple;

            float ContactThreshold = 0.05f;
            RigidBody rigBody;
            Vector3 normal, A;
            if (couple.faceOnA)
            {
                normal = couple.rigBodyA.rotation * couple.definingFace.normal;
                normal.Normalize();
                A = couple.rigBodyA.rotation * couple.definingFace.vertices[0].Position + couple.rigBodyA.position;
                rigBody = couple.rigBodyB;
            }
            else
            {
                normal = couple.rigBodyB.rotation * couple.definingFace.normal;
                normal.Normalize();
                A = couple.rigBodyB.rotation * couple.definingFace.vertices[0].Position + couple.rigBodyB.position;
                rigBody = couple.rigBodyA;
            }

            for (int i = 0; i < rigBody.numberOfPolygons; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    //A er en punkt i planet som testas, B är punkten man testar imot 
                    Vector3 B = rigBody.rotation * rigBody.polygonArray[i].vertices[j].Position + rigBody.position;
                    Vector3 AB = B - A;
                    float test = Math.Abs(Vector3.Dot(AB, normal));
                    Boolean hej = contact.ContactPoints.Contains(B);
                    if (Math.Abs(Vector3.Dot(AB, normal)) < ContactThreshold && !contact.ContactPoints.Contains(B) )
                    {
                        Console.WriteLine("Skalären: " + Math.Abs(Vector3.Dot(AB, normal)));
                        contact.ContactPoints.Add(B);
                    }
                }
            }
            contact.normal = normal;
            return contact;
        }

        public Boolean checkSeparatingPlane(ref Couple couple)
        {
            int rightSideCheck = 0;
            RigidBody rigBody;
            Vector3 normal, A;
            if (couple.faceOnA)
            {
                normal = couple.rigBodyA.rotation * couple.definingFace.normal;
                normal.Normalize();
                A = couple.rigBodyA.rotation * couple.definingFace.vertices[0].Position + couple.rigBodyA.position;
                rigBody = couple.rigBodyB;
            }
            else
            {
                normal = couple.rigBodyB.rotation * couple.definingFace.normal;
                normal.Normalize();
                A = couple.rigBodyB.rotation * couple.definingFace.vertices[0].Position + couple.rigBodyB.position;
                rigBody = couple.rigBodyA;
            }

            for (int i = 0; i < rigBody.numberOfPolygons; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    //A er en punkt i planet som testas, B är punkten man testar imot 
                    Vector3 B = rigBody.polygonArray[i].vertices[j].Position + rigBody.position;
                    Vector3 AB = B - A;
                    if (Vector3.Dot(AB, normal) > 0)// no collision
                    { 
                        rightSideCheck++;
                    }
                }
            }
            if (rightSideCheck == rigBody.numberOfPolygons * 3) //antalet vertexar i hela rigbodyn //separationsplan!!!
            { 
                //collisionState = NOTHINGTOWORRYABOUT;
                return true;
            }
            collisionState = COLLISION;
            return false;
        }
    }
}

    

